/*
 * 文件名: main.cpp
 * 描述: 这个文件实现了ECG、ACC、Breathe、PPG采集功能
 * 采样率：
 * ECG：256Hz
 * ACC：50HZ
 * breathe：38Hz
 * PPG：100Hz
 * 作者: lzy
 * 创建日期: 2025-06
 * 版本: 1.0
 * 修改历史:
 *   2025-06 - 初始版本
 */
#include <Arduino.h>
#include <sstream>
#include <string>
#include <app/FDC2214.h>
#include <main.h>
#include <FreeRTOS/FreeRTOS.h>
#include <FreeRTOS/task.h>
#include <FreeRTOS/queue.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <ESP32Time.h>
#include "app/init_peripheral.h"
#include "app/AsyncLogger.h"
#include "app/low_energy.h"
#include "algorithm/filter.h"
#include "algorithm/algorithm.h"
#include "algorithm/algo_ecg_calculate_hr.h"
#include "algorithm/HeartRateCalibrator.h"
#include "algorithm/calculateSpo2.h"
#include "algorithm/SpO2Calibrator.h"
#include "algorithm/RespiratoryRateCalculator.h"
#include "dataset/ecgdata.h"
#include "app/bat_manager.h"

void task_message_send(sys_event_t event);
void usr_ble_ecg_data_send(sys_event_t sys_event, uint8_t* data, uint16_t size);
void usr_ble_acc_data_send(sys_event_t sys_event, float* fdata);
void usr_ble_breathe_data_send(sys_event_t sys_event, uint8_t * data, uint16_t size);
void usr_ble_ppg_data_send(sys_event_t sys_event, uint8_t * data, uint16_t size);
void print_wakeup_reason(void);
void enterSleepMode(void);

bool ble_active = true; // 蓝牙是否处于活动状态

RespiratoryRateCalculator& calculator = RespiratoryRateCalculator::getInstance();
uint32_t resp_sample_index = 0;
static UBaseType_t myUBaseType;
// 获取心率校准器实例
HeartRateCalibrator* calibrator = HeartRateCalibrator_getInstance();
// 校准后的心率、血氧值、呼吸率
uint32_t DateAndTime;
uint32_t calibrated_hr = 0;
uint32_t calibrated_so2 = 0;
uint32_t calibrated_resp = 0;
// 关机全局变量
bool wearingDetection = false;
bool deviceShouldSleep = false;
bool deviceSleepEnable = false;
unsigned long sleepTriggerTime = 0;
/**
 * 初始化ESP32 TIMER
 */
ESP32Time rtc(0*3600);  // offset in seconds GMT+0
static struct tm timeinfo;
/**
 * @brief 获取RTC时间
 * tm_year : bit31-bit26
 * tm_mday : bit25-bit21
 * tm_mon  : bit20-bit17 
 * tm_hour : bit16-bit12
 * tm_min  : bit11-bit6
 * tm_sec  : bit5-bit0
 * @return uint32_t 
 */
// 从时间戳设置RTC
void setRTCFromTimestamp(unsigned long timestamp) {
  // 将Unix时间戳转换为tm结构
  time_t timeSec = timestamp;
  struct tm *_timeinfo = localtime(&timeSec);
  // 设置RTC
  timeinfo.tm_year = _timeinfo->tm_year;
  timeinfo.tm_mon  = _timeinfo->tm_mon;
  timeinfo.tm_mday = _timeinfo->tm_mday;
  timeinfo.tm_hour = _timeinfo->tm_hour;
  timeinfo.tm_min  = _timeinfo->tm_min;
  timeinfo.tm_sec  = _timeinfo->tm_sec;
  // 更新RTC
  rtc.setTime(timestamp);
}
// 打印北京时间（UTC+8）
void printBeijingTime() {
   time_t now = rtc.getEpoch(); // 获取UTC时间戳
   now += 8 * 3600; // 手动加上8小时
  
   struct tm timeinfo;
   gmtime_r(&now, &timeinfo); // 使用gmtime_r（线程安全）避免localtime的时区干扰
  
   char timeStr[64];
   strftime(timeStr, sizeof(timeStr), "%Y-%m-%d %H:%M:%S", &timeinfo);
   Serial.printf("Beijing Time: %s\n", timeStr);
}
void printTimestamp() {
	Serial.printf("UTC Timestamp: %lu\n", rtc.getEpoch());
}

/**
 * 初始化BLE
 */
uint8_t txValue = 0;
BLEServer *pServer = NULL;					 // BLEServer指针 pServer
BLECharacteristic *pTxCharacteristic1 = NULL; // BLECharacteristic指针 pTxCharacteristic
BLECharacteristic *pTxCharacteristic2 = NULL; // BLECharacteristic指针 pTxCharacteristic_2
BLECharacteristic *pTxCharacteristic3 = NULL; // BLECharacteristic指针 pTxCharacteristic_3
BLECharacteristic *pTxCharacteristic4 = NULL; // BLECharacteristic指针 pTxCharacteristic_4
bool deviceConnected = false;				 // 本次连接状态
bool oldDeviceConnected = false;			 // 上次连接状态
// See the following for generating UUIDs: https://www.uuidgenerator.net/
#define SERVICE_UUID "8b3bd5f2-d9f6-4627-8d4a-95528b20f5ed" // UART service UUID
#define CHARACTERISTIC_UUID_RX "33bd79a8-fc9a-4201-b682-2b029130b90e"
#define CHARACTERISTIC_UUID_TX1 "c48d0417-de7a-41ef-b34c-1c1f0202a53f"
#define CHARACTERISTIC_UUID_TX2 "c48d0418-de7a-41ef-b34c-1c1f0202a53f"
#define CHARACTERISTIC_UUID_TX3 "c48d0419-de7a-41ef-b34c-1c1f0202a53f"
#define CHARACTERISTIC_UUID_TX4 "c48d0420-de7a-41ef-b34c-1c1f0202a53f"
class MyServerCallbacks : public BLEServerCallbacks
{
	void onConnect(BLEServer *pServer)
	{
		deviceConnected = true;
	};
	void onDisconnect(BLEServer *pServer)
	{
		deviceConnected = false;
	}
};
class MyCallbacks : public BLECharacteristicCallbacks
{
	void onWrite(BLECharacteristic *pCharacteristic)
	{
		std::string rxValue = pCharacteristic->getValue(); // 接收信息
		if (rxValue.length() > 0)
		{ 
      		// 向串口输出收到的值
			// Serial.print("RX: ");
			// for (int i = 0; i < rxValue.length(); i++)
			// {
			// 	Serial.print(rxValue[i]);
			// }

			unsigned long timestamp = std::stol(rxValue);
			asyncPrint("%s\r\n",rxValue);
    		setRTCFromTimestamp(timestamp);

			// 关闭蓝牙
			ble_active = false; // 设置蓝牙不活跃
			//deviceConnected = false; // 设置设备连接状态为false
			RFActivityStatus = FIELD_FALLING;// 场下降，开启NFC写入
			BLEDevice::deinit(true);
		}
	}
};

void BLE_Init(void)
{
	// 创建一个 BLE 设备
	BLEDevice::init("UART_BLE");

	// 创建一个 BLE 服务
	pServer = BLEDevice::createServer();
	pServer->setCallbacks(new MyServerCallbacks()); // 设置回调
	BLEService *pService = pServer->createService(SERVICE_UUID);

	// 创建一个 BLE 特征
	pTxCharacteristic1 = pService->createCharacteristic(CHARACTERISTIC_UUID_TX1, BLECharacteristic::PROPERTY_NOTIFY);//ecg数据
	pTxCharacteristic1->addDescriptor(new BLE2902());
	pTxCharacteristic2 = pService->createCharacteristic(CHARACTERISTIC_UUID_TX2, BLECharacteristic::PROPERTY_NOTIFY);//心率、加速度、温度
	pTxCharacteristic2->addDescriptor(new BLE2902());
	pTxCharacteristic3 = pService->createCharacteristic(CHARACTERISTIC_UUID_TX3, BLECharacteristic::PROPERTY_NOTIFY);
	pTxCharacteristic3->addDescriptor(new BLE2902());
	pTxCharacteristic4 = pService->createCharacteristic(CHARACTERISTIC_UUID_TX4, BLECharacteristic::PROPERTY_NOTIFY);
	pTxCharacteristic4->addDescriptor(new BLE2902());
	BLECharacteristic *pRxCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_RX, BLECharacteristic::PROPERTY_WRITE);
	pRxCharacteristic->setCallbacks(new MyCallbacks()); // 设置回调

	pService->start();					// 开始服务
	pServer->getAdvertising()->start(); // 开始广播
	Serial.println(" 等待一个客户端连接，且发送通知... ");
}
void BLE_Connection_Status(void)
{
	// deviceConnected 已连接
	if (deviceConnected)
	{
		#if 0
		pTxCharacteristic1->setValue(&txValue, 1); // 设置要发送的值为1
		pTxCharacteristic1->notify();			  // 广播
		txValue++;								  // 指针地址自加1
												  // delay(2000);                           // 如果有太多包要发送，蓝牙会堵塞
		#endif
	}
	// disconnecting  断开连接
	if (!deviceConnected && oldDeviceConnected)
	{
		delay(500);					 // 留时间给蓝牙缓冲
		pServer->startAdvertising(); // 重新广播
		Serial.println(" 开始广播 ");
		oldDeviceConnected = deviceConnected;
	}
	// connecting  正在连接
	if (deviceConnected && !oldDeviceConnected)
	{
		// do stuff here on connecting
		oldDeviceConnected = deviceConnected;
	}
}

// 创建一个互斥信号量和队列
static SemaphoreHandle_t MuxSem_Handle = xSemaphoreCreateMutex();
static QueueHandle_t sys_queue;
/**
 * @brief 获取ECG数据并发送到BLE
 */
static void xTask_ecg(void *xTask)
{
	sys_event_t sys_event;
	float ecg_samples[32]; // 直接存储浮点数
	while (1)
	{
		for(int i=0; i<32; i++)
		{
			max30003.getEcgSamples();
			ecg_samples[i] = max30003.ecgdata;
		}
		usr_ble_ecg_data_send(sys_event, (uint8_t*)ecg_samples, sizeof(ecg_samples));
		vTaskDelay(pdMS_TO_TICKS(5)); // 非阻塞，释放CPU给其他任务
	}
}

/**
 * @brief 获取加速度数据，并发送到BLE
 */
static void xTask_acc(void *xTask)
{
	sys_event_t sys_event;
	float sendBuf[4];
	while (1)
	{
		if (accel.available())
		{
			sendBuf[0] = accel.getX();
			sendBuf[1] = accel.getY();
			sendBuf[2] = accel.getZ();
			sendBuf[3] = accel.getTemperature();
			//asyncPrint("%.2f\r\n",sendBuf[0]);
			usr_ble_acc_data_send(sys_event, sendBuf);
		}
		vTaskDelay(pdMS_TO_TICKS(10)); // 非阻塞，释放CPU给其他任务
	}
}

#define CHAN_COUNT 1 
#define BREATHE_FR 38
static void xTask_resp(void *xTask)
{
	sys_event_t sys_event;
	uint32_t breathe_buf[BREATHE_FR];
	uint8_t index = 0; // breathe_buf index
	unsigned long capa[CHAN_COUNT]; // variable to store data from FDC
	while(1)
	{
		for (int i = 0; i < CHAN_COUNT; i++){ // for each channel
		  	capa[i] = capsense.getReading28(i);//  
	
			//asyncPrint("%d\r\n",capa[i]);
			breathe_buf[index] = capa[i];
		}
		index++;
		if (index >= BREATHE_FR) {
			index = 0;
			usr_ble_breathe_data_send(sys_event, (uint8_t*)breathe_buf, sizeof(breathe_buf));
		}
		vTaskDelay(pdMS_TO_TICKS(10)); // 非阻塞，释放CPU给其他任务
	}
}

#define calcalate_ppg 500
uint32_t bufferindex = 0;
uint32_t irBuffer[calcalate_ppg]; //infrared LED sensor data
uint32_t redBuffer[calcalate_ppg];  //red LED sensor data
int32_t bufferLength = 0; //data length
int32_t spo2; //SPO2 value
int8_t validSPO2; //indicator to show if the SPO2 calculation is valid
int32_t heartRate; //heart rate value
int8_t validHeartRate; //indicator to show if the heart rate calculation is valid
bool spo2_calculate = false;

long startTime;
long samplesTaken = 0; //Counter for calculating the Hz or read rate
static void xTask_ppg(void *xTask)
{
  static int PPG_Index = 0;
  uint32_t buff[50];
  sys_event_t sys_event;
  while(1)
  {
	_particleSensor.check(); //Check the sensor, read up to 3 samples
    if (_particleSensor.available()) //do we have new data?
    {
      samplesTaken++;
	  uint32_t red = _particleSensor.getFIFORed();
	  uint32_t ir  = _particleSensor.getFIFOIR();
	  uint32_t green = _particleSensor.getFIFOGreen();
      float Hz = (float)samplesTaken / ((millis() - startTime) / 1000.0);

      //asyncPrint("R[%d]  ", red);
	  //asyncPrint("I[%d]  ", ir);
	  //asyncPrint("G[%d]  ", green);
	  //asyncPrint("Hz[%.2f]\r\n", Hz);

	  buff[PPG_Index++] = red;
	  buff[PPG_Index++] = ir;
	  if (PPG_Index >= 50) {
		PPG_Index = 0;
		usr_ble_ppg_data_send(sys_event, (uint8_t*)buff, sizeof(buff));
	  }
      _particleSensor.nextSample(); //We're finished with this sample so move to next sample
    }
    vTaskDelay(pdMS_TO_TICKS(1));
  }
}

uint32_t cal_ppg_index = 0;
uint32_t cal_redBuffer[100];
uint32_t cal_irBuffer[100];
/**
 * @brief 处理ECG数据，计算心率，并发送到BLE
 */
static void xTask_queue(void *xTask)
{
	sys_queue = xQueueCreate(120, sizeof(sys_event_t));
	static sys_event_t event;
	float sendECG[32];
	uint16_t ecg_calculate_index = 0;
	boolean hr_calculate_flag = false;//计算心率标志位
	float sendBreathe[BREATHE_FR];
	while(1)
	{
 		if(xQueueReceive(sys_queue, (void *)&event, (TickType_t)(portMAX_DELAY)))
        {
        	switch(event.type)
        	{
        	  	case BLE_SEND_ECG_DATA:
        		{
					for(int i = 0; i<(event.size/4); i++) 
					{
						float ecg = 0;
						memcpy(&ecg, &event.ecgdata[4*i], 4); // 复制 4 字节到 float

						// float dECG = ecgdata_g[ecgdata_index++];
                        // if(ecgdata_index >= 1625)
                        // {
                        //    ecgdata_index = 0;
                        // }
						float dECG = filter(ecg);
						sendECG[i] = dECG;
					}
					if(ble_active){
						pTxCharacteristic1->setValue((uint8_t*)&sendECG, event.size);
						pTxCharacteristic1->notify();
					}

					sys_event_t sys_event;
					sys_event.type = MSG_RESP_CALCULATE;
					sys_event.size = 32;
					sys_event.Fdata = sendECG;
					task_message_send(sys_event);
        		}break;
				case MSG_RESP_CALCULATE:{
					for(uint8_t i=0; i<event.size; i++){
						calculator.addData(event.Fdata[i], resp_sample_index++);	
						ecg_calculate_index++;
 						if(ecg_calculate_index == 2*256){
                        	ecg_calculate_index = 0;
                       		hr_calculate_flag = true;
                    	}
					}
					if(hr_calculate_flag==true)
					{
						hr_calculate_flag = false;
						std::stringstream ss;
						std::string buf;
						calibrated_hr = HeartRateCalibrator_calibrate(calibrator, calculator.getHeartRate());
						calibrated_resp = calculator.getRespiratoryRate();
    					ss << "hr:" << calibrated_hr << ",resp:" << calibrated_resp;
    					buf = ss.str();
						if(ble_active){
							pTxCharacteristic2->setValue((uint8_t*)buf.c_str(), buf.length());
							pTxCharacteristic2->notify();
						}
					}	
				}break;
				case BLE_SEND_ACC_DATA:
        		{
					std::stringstream ss;
					std::string buf;
    				ss << "accx:" << event.Fdata[0] << ",accy:" << event.Fdata[1] << ",accz:" << event.Fdata[2] << ",tempc:"<<event.Fdata[3];
    				buf = ss.str();
					if(ble_active){
						pTxCharacteristic2->setValue((uint8_t*)buf.c_str(), buf.length());
						pTxCharacteristic2->notify();
					}
        		}break;
				case BLE_SEND_BREATHE_DATA:{
					for(int i = 0; i<(event.size/4); i++) 
					{
						uint32_t value = 0;
						memcpy(&value, &event.ecgdata[4*i], 4);
						sendBreathe[i] = (float)value;
					}
					if(ble_active){
						pTxCharacteristic3->setValue((uint8_t*)sendBreathe, event.size);
						pTxCharacteristic3->notify();
					}
				}break;
				case BLE_SEND_PPG_DATA:{
					if(ble_active){
						pTxCharacteristic4->setValue(event.ecgdata, event.size);
						pTxCharacteristic4->notify();
					}

					for(int m=0; m<(event.size/8); m++)
					{
						uint32_t red = 0;
						uint32_t ir  = 0;
						memcpy(&red, &event.ecgdata[8*m], 4); // 复制4字节到uint32_t
						memcpy(&ir, &event.ecgdata[8*m+4], 4);
						if(cal_ppg_index < 100)
						{
							cal_redBuffer[cal_ppg_index] = red;
							cal_irBuffer[cal_ppg_index] = ir;
							cal_ppg_index++;
						}
					}
					if(cal_ppg_index >= 100)
					{
						cal_ppg_index = 0;
						for(int i=100; i<500; i++)
        				{
            				redBuffer[i-100] = redBuffer[i];
            				irBuffer[i-100]  = irBuffer[i];
						}
						for(int i=400; i<500; i++)
        				{
							if(bufferLength < 500)
							{
								bufferLength++;
							}
            				redBuffer[i] = cal_redBuffer[i-400];
            				irBuffer[i]  = cal_irBuffer[i-400];
							if(cal_irBuffer[i-400] > 5000) // 手指放开时一般都大于5000
							{
								spo2_calculate = true;
								wearingDetection = true; // 戴上手指
							}
							else{
								spo2_calculate = false;
								wearingDetection = false; // 手指放开
							}
						}
					
						if(spo2_calculate && bufferLength>=500)// 防止误判
						{
							maxim_heart_rate_and_oxygen_saturation1(irBuffer, bufferLength, redBuffer, &spo2, &validSPO2, &heartRate, &validHeartRate);
							//asyncPrint("SPO2: %d, validSPO2: %d, HeartRate: %d, validHeartRate: %d\r\n", spo2, validSPO2, heartRate, validHeartRate);
						}
						if(spo2_calculate && spo2<100 && spo2>70) // 划定显示区间, 光学会受自然光影响
						{
							calibrated_so2 = spo2;
						}
						else
						{
							calibrated_so2 = 0;
						}
						if(bufferLength>=500){
							std::stringstream ss;
							std::string buf;
    						ss << "SO2:" << calibrated_so2;
    						buf = ss.str();
							if(ble_active){
								pTxCharacteristic2->setValue((uint8_t*)buf.c_str(), buf.length());
								pTxCharacteristic2->notify();
							}
						}
					}
				}break;
				case MSG_BATTERY_PERCENTAGE:{
					float battery_voltage = readADC_Avg();
					uint8_t percent = calculate_battery_percentage(battery_voltage);
					asyncPrint("Battery Voltage: %.2f V, %d\r\n", battery_voltage, percent);

					std::stringstream ss;
					std::string buf;
					ss << "Battery:" << (int)percent;
					buf = ss.str();
					if(ble_active){
						pTxCharacteristic2->setValue((uint8_t*)buf.c_str(), buf.length());
						pTxCharacteristic2->notify();
					}
				}break;
				case MSG_REC_NFC_DETECT:{
					if (button1.pressed) {
        				asyncPrint("Button 1 has been pressed %u times\n", button1.numberKeyPresses);
        				button1.pressed = false;
						RFActivity = 1;
					}
					/* 检测NFC有没有中断 */
					if(RFActivity)
					{
						RFActivity = 0;
						uint8_t ITStatus = 0;
						tag.readEEPROM(ST25DV_ITSTS_DYN_REG, &ITStatus, 1);
						if(ITStatus&0x10)
						{
							RFActivityStatus = FIELD_RISING;
							asyncPrint("Detected NFC FIELD_RISING");
						}else if(ITStatus&0x08)
						{
							RFActivityStatus = FIELD_FALLING;
							asyncPrint("Detected NFC FIELD_FALLING");
						}
					}
				}break;
				case MSG_REC_NFC_DATA:{
					if(RFActivityStatus == FIELD_FALLING)
					{
						if(nfc_write_notify == true){
							DateAndTime = rtc.getEpoch(); // 返回Unix时间戳
  							asyncPrint("%d  timestamp: %d\n", LastSamplePointer, DateAndTime);

							if(!tag.writeEEPROM(LastSamplePointer, (uint8_t *)&DateAndTime, 4))
							{
								asyncPrint("write error");
								break;
							}

  							tag.writeEEPROM(LAST_ADDRESS, (uint8_t *)&LastSamplePointer, 4);//写最新的地址;

							LastSamplePointer += 4;
							uint8_t hr_t  = calibrated_hr;
							uint8_t so2_t = calibrated_so2;
							uint8_t resp_t= calibrated_resp; 
							uint8_t CompactData[4] = {hr_t, so2_t, resp_t, 0x00};
							if(!tag.writeEEPROM(LastSamplePointer, CompactData, 4))
							{
								asyncPrint("write error");
								break;
							}
							asyncPrint("%d, %x  %x  %x  %x\r\n",LastSamplePointer, CompactData[0], CompactData[1], CompactData[2], CompactData[3]);
							LastSamplePointer += 4;
							/* 如果超过最大EEPROM size，则从起点写入 */
							if (LastSamplePointer >= SMARTAG_END_ADDR_COMPACT_DATA) {
								asyncPrint("\r\n\r\n!!Reached the End of the TAG\r\n\toverwriting oldest samples!!\r\n\r\n");
    							LastSamplePointer = SMARTAG_BEGIN_ADDR_COMPACT_DATA; 
							}
							nfc_write_notify = false;

							// 关机指令模式
							uint32_t mode = 0;
							tag.readEEPROM(WORKING_MODE, (uint8_t*)&mode, 4);
							asyncPrint("Mode: %x\r\n", mode);
							if(mode == 0xaa)// 关机指令模式
							{
								deviceShouldSleep = true;
							}
						}
					}
				}break;
        	}
        }
		vTaskDelay(pdMS_TO_TICKS(1)); // 非阻塞，释放CPU给其他任务
	}
}

TimerHandle_t timer_100ms;
#define TIMER_PERIOD_100MS pdMS_TO_TICKS(100)
static void timer_100ms_handle(TimerHandle_t xTimer)
{
	static sys_event_t sys_event;
    static uint32_t cnt = 0;
	cnt++;
	if(cnt%10 == 0)
	{
		// 定时关机
		if(deviceSleepEnable == true)
		{
			sleepTriggerTime++;
			asyncPrint("Sleep Trigger Time: %d seconds\r\n", sleepTriggerTime);
			if(sleepTriggerTime >= SLEEP_DELAY_SECONDS)
			{
				deviceShouldSleep = true;
			}
		}

		// NFC场检测
		//sys_event.type = MSG_REC_NFC_DETECT;
		//task_message_send(sys_event);

		if(cnt%100==0){//每10写入一次NFC
			nfc_write_notify = true; // 每10次定时器触发，设置NFC写入通知为true
			// 发送NFC写入事件
			sys_event.type = MSG_REC_NFC_DATA;
			task_message_send(sys_event);

			// 发送电池电量
			sys_event.type = MSG_BATTERY_PERCENTAGE;
			task_message_send(sys_event);
		}
	}
}

void setup() {
	Serial.begin(115200);
	setupAsyncPrint();
	print_wakeup_reason();
	disableAllUnusedPeripherals();

 	// 配置ADC
  	analogReadResolution(12); // 设置ADC分辨率为12位 (0-4095)
  	analogSetAttenuation(ADC_11db); // 设置衰减为11dB (量程约0-3.3V)

	init_power();
	max30003_Init();
	lis2dh12_init();
	st25dv64_init();
	init_max30101();
	fdc2214_init();

	startTime = millis();

	// 读取关机前记录的时间戳和地址
	uint32_t mode = 0x00;
	tag.readEEPROM(LAST_ADDRESS, (uint8_t*)&LastSamplePointer, 4);
	tag.writeEEPROM(WORKING_MODE, (uint8_t*)&mode, 4);// 重置

	BLE_Init();
	timer_100ms = xTimerCreate("TIMER_10HZ", TIMER_PERIOD_100MS, pdTRUE, NULL, timer_100ms_handle);
    xTimerStart(timer_100ms, 0);
}

void loop() {
  	xTaskCreate(xTask_queue, "xTaskFive", 1024*8, NULL, 6, NULL);
	xTaskCreate(xTask_ecg, "TaskOne", 1024*4, NULL, 6, NULL);	   
	xTaskCreate(xTask_acc, "xTaskTwo", 1024*4, NULL, 6, NULL);	
	xTaskCreate(xTask_resp, "xTaskThree", 1024*4, NULL, 6, NULL);	
	xTaskCreate(xTask_ppg, "xTaskFour", 1024*4, NULL, 6, NULL);	
  while (1)
  {
    /* code */
    BLE_Connection_Status();
	if(deviceConnected == false)
	{
		// 关机计时启动
		deviceSleepEnable = true;
	}
	else{
		deviceSleepEnable = false;
		sleepTriggerTime = 0;
	}
	if(deviceShouldSleep == true)
	{
		enterSleepMode();
	}
  }
}

void task_message_send(sys_event_t event)
{
	// 尝试获取互斥锁（10ms超时）
    if (xSemaphoreTake(MuxSem_Handle, pdMS_TO_TICKS(10)) == pdTRUE) {
        if (sys_queue != NULL) {
            // 根据队列剩余空间动态调整等待时间
			myUBaseType = uxQueueSpacesAvailable(sys_queue);
            TickType_t waitTime = (myUBaseType > 5) ? 0 : pdMS_TO_TICKS(5);
			//asyncPrint("spaces: %d, wait time: %d ms", myUBaseType, waitTime);
            if (xQueueSend(sys_queue, &event, waitTime) != pdTRUE) {
                // 记录发送失败（可统计丢包率或触发错误处理）
                asyncPrint("Queue full, event dropped!");
            }
        }
        xSemaphoreGive(MuxSem_Handle);
    } else {
        // 记录锁获取失败
        asyncPrint("Mutex timeout!");
    }
}
/**
 * @brief 发送ECG数据到BLE
 */
void usr_ble_ecg_data_send(sys_event_t sys_event, uint8_t* data, uint16_t size)
{
	sys_event.type = BLE_SEND_ECG_DATA;
	sys_event.ecgdata = data;
	sys_event.size = size;
	task_message_send(sys_event);
}
/**
 * @brief 发送加速度数据到BLE
 */
void usr_ble_acc_data_send(sys_event_t sys_event, float* fdata)
{
	sys_event.type = BLE_SEND_ACC_DATA;
	sys_event.Fdata = fdata;
	task_message_send(sys_event);
}
/**
 * @brief 发送呼吸数据到BLE
 */
void usr_ble_breathe_data_send(sys_event_t sys_event, uint8_t *data, uint16_t size)
{
	sys_event.type = BLE_SEND_BREATHE_DATA;
	sys_event.ecgdata = data;
	sys_event.size = size;
	task_message_send(sys_event);
}
/**
 * @brief 发送PPG数据到BLE
 */
void usr_ble_ppg_data_send(sys_event_t sys_event, uint8_t *data, uint16_t size)
{
	sys_event.type = BLE_SEND_PPG_DATA;
	sys_event.ecgdata = data;
	sys_event.size = size;
	task_message_send(sys_event);
}

// 打印唤醒原因函数
void print_wakeup_reason(void) {
  esp_sleep_wakeup_cause_t wakeup_reason = esp_sleep_get_wakeup_cause();
  
  switch(wakeup_reason) {
    case ESP_SLEEP_WAKEUP_EXT0: 
      asyncPrint("ESP_SLEEP_WAKEUP_EXT0\r\n");
      break;
    case ESP_SLEEP_WAKEUP_EXT1:
      asyncPrint("ESP_SLEEP_WAKEUP_EXT1\r\n");
      break;
    case ESP_SLEEP_WAKEUP_TIMER: 
      asyncPrint("ESP_SLEEP_WAKEUP_TIMER\r\n"); 
      break;
    case ESP_SLEEP_WAKEUP_TOUCHPAD: 
      asyncPrint("ESP_SLEEP_WAKEUP_TOUCHPAD\r\n"); 
      break;
    case ESP_SLEEP_WAKEUP_ULP: 
      asyncPrint("ESP_SLEEP_WAKEUP_ULP\r\n"); 
      break;
    default: 
      asyncPrint("default\r\n");
      break;
  }

  // 配置GPIO13为上拉输入（可选，根据实际电路设计）
  pinMode(WAKEUP_PIN, INPUT_PULLDOWN);
}

/**
 * @brief 停止所有任务
 */
void stopAllTasks() {
    TaskHandle_t taskHandles[5]; // 假设有4个任务
    
    // 获取所有任务句柄
    taskHandles[0] = xTaskGetHandle("TaskOne");
    taskHandles[1] = xTaskGetHandle("TaskTwo");
    taskHandles[2] = xTaskGetHandle("xTaskThree");
    taskHandles[3] = xTaskGetHandle("xTaskFour");
	taskHandles[4] = xTaskGetHandle("xTaskFive");
    // 删除所有任务
    for(int i = 0; i < 5; i++) {
        if(taskHandles[i] != NULL) {
            vTaskDelete(taskHandles[i]);
        }
    }
    // 删除定时器
    if(timer_100ms != NULL) {
        xTimerDelete(timer_100ms, 0);
    }
}
void deinitBLE() {
    if(pServer != NULL) {
        pServer->disconnect(pServer->getConnId()); // 断开连接
		pServer->getAdvertising()->stop(); // 停止广播
        BLEDevice::deinit(true); // true 表示释放内存
    }
}

void enterSleepMode(void)
{
  // 配置EXT0唤醒源 - GPIO13低电平唤醒
  // 参数1: 唤醒引脚(GPIO编号)
  // 参数2: 唤醒电平(0=低电平, 1=高电平)

  tag.writeEEPROM(LAST_ADDRESS, (uint8_t *)&LastSamplePointer, 4);//写最新的地址;
  
  stopAllTasks();
  //deinitBLE();
  esp_sleep_enable_ext0_wakeup((gpio_num_t)WAKEUP_PIN, 1);// 设置唤醒引脚
  deinit_power();// 关闭供电
  delay(SLEEP_DELAY_MS);
  esp_deep_sleep_start();
}
